#include <iostream>
#include <string>
#include <fstream>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <tf/LinearMath/Quaternion.h>
#include <tf/transform_datatypes.h>

// using namespace std;


nav_msgs::Path odomPath;


std::string root_dir = ROOT_DIR;    // 设置根目录，在CMakeLists.txt中设置的，${CMAKE_CURRENT_SOURCE_DIR}/，即CMakeLists.txt所在的目录

const std::string filePathDirectory = std::string(std::string(ROOT_DIR)) + "log/";  // 保存文件的文件夹
/************************* txt files *******************************/
const std::string fileName = "OdomRecored.txt";          // 保存文件的txt文件名
std::string savePathFilesDirectory = filePathDirectory + fileName;  // 保存在../log中
int reflash = system((std::string("exec rm ") + savePathFilesDirectory).c_str());   // 删除原来的文件
std::ofstream Pathfiles(savePathFilesDirectory);    // 重新建立文件

/************************* csv files *******************************/
const std::string csvfileName = "OdomCSV.csv";          // 保存文件的csv文件名
std::string saveCSVFilesDirectory = filePathDirectory + csvfileName; // 保存在home下的/savePCDMap/log中
int csvreflash = system((std::string("exec rm ") + saveCSVFilesDirectory).c_str());   // 删除原来的文件
std::ofstream CSVfiles(saveCSVFilesDirectory);    // 重新建立文件


template<typename T>
void odomRPY2rosRPY(nav_msgs::Odometry *thisOdomMsg, T *rosRoll, T *rosPitch, T *rosYaw)
{
    double odomRoll, odomPitch, odomYaw;
    tf::Quaternion orientation;
    tf::quaternionMsgToTF(thisOdomMsg->pose.pose.orientation, orientation);
    tf::Matrix3x3(orientation).getRPY(odomRoll, odomPitch, odomYaw);

    *rosRoll = odomRoll;
    *rosPitch = odomPitch;
    *rosYaw = odomYaw;
}



void Odometry_handle(const nav_msgs::Odometry::ConstPtr& msgIn)
{
    nav_msgs::Odometry data = *msgIn;


    /*********************** record the data of odom ********************************/
    static double time_init = data.header.stamp.toSec();
    auto timeStart = ros::Time().fromSec(time_init);

    // 保存为TUM的txt格式
    Pathfiles.open(savePathFilesDirectory, std::ios::app);
    if (Pathfiles.is_open())
    {
        Pathfiles << std::fixed << data.header.stamp - timeStart
                  << " "  << data.pose.pose.position.x
                  << " "  << data.pose.pose.position.y
                  << " "  << data.pose.pose.position.z
                  << " "  << data.pose.pose.orientation.x
                  << " "  << data.pose.pose.orientation.y
                  << " "  << data.pose.pose.orientation.z
                  << " "  << data.pose.pose.orientation.w  
                  << std::endl;
        Pathfiles.close();
    }
    else
    {
        ROS_ERROR("Failed to open the txt file for writing.");
    }


    // 保存为CSV的格式
    double roll, pitch, yaw;
    odomRPY2rosRPY(&data, &roll, &pitch, &yaw);

    CSVfiles.open(saveCSVFilesDirectory, std::ios::app);
    if (CSVfiles.is_open())
    {
        CSVfiles << std::fixed << data.header.stamp - timeStart
                 << ","  << data.pose.pose.position.x
                 << ","  << data.pose.pose.position.y
                 << ","  << data.pose.pose.position.z
                 << ","  << data.pose.pose.orientation.x
                 << ","  << data.pose.pose.orientation.y
                 << ","  << data.pose.pose.orientation.z
                 << ","  << data.pose.pose.orientation.w
                 << ","  << roll
                 << ","  << pitch
                 << ","  << yaw
                 << ","  << data.twist.twist.linear.x
                 << ","  << data.twist.twist.linear.y
                 << ","  << data.twist.twist.linear.z
                 << ","  << data.twist.twist.angular.x
                 << ","  << data.twist.twist.angular.y
                 << ","  << data.twist.twist.angular.z
                 << std::endl;
        CSVfiles.close();
    }
    else
    {
        ROS_ERROR("Failed to open the csv file for writing.");
    }



}




/*****************************************************************/
void testRPY2rosQuat()
{
    double Roll, Pitch, Yaw;
    Roll = 0;
    Pitch = 0;
    Yaw = 1;
    tf::Quaternion q;
    q = tf::createQuaternionFromRPY(Roll, Pitch, Yaw);
    std::cout << "q.w = " << q.w() << " "
                << "q.x = " << q.x() << " "
                  << "q.y = " << q.y() << " "
                    << "q.z = " << q.z() << std::endl;
}

void testQuat2rosRPY()
{
    double Roll, Pitch, Yaw;
    tf::Quaternion orientation;
    
    nav_msgs::Odometry data;
    data.pose.pose.orientation.w = 0.786067;
    data.pose.pose.orientation.x = 0.167519;
    data.pose.pose.orientation.y = 0.570941;
    data.pose.pose.orientation.z = 0.167519;

    tf::quaternionMsgToTF(data.pose.pose.orientation, orientation);
    tf::Matrix3x3(orientation).getRPY(Roll, Pitch, Yaw);

    std::cout << "Roll = " << Roll << " "
                << "Pitch = " << Pitch << " "
                    << "Yaw = " << Yaw << std::endl;
}
/*****************************************************************/



int main(int argc, char** argv)
{
    ros::init(argc, argv, "lio_sam_recorder");
    ros::NodeHandle nh;
    testRPY2rosQuat();
    testQuat2rosRPY();

    ros::Subscriber sub_liosamPath = nh.subscribe<nav_msgs::Odometry>
                                    ("leg_odom", 1000, &Odometry_handle, ros::TransportHints().tcpNoDelay());

    ros::Rate rate(5000);
    while (ros::ok())
    {
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}